Robot control apparatus, and method and program for obtaining angle of joint of robot

ABSTRACT

In a method of obtaining an angle of each joint of a 6-axis vertical articulated robot when a position and a posture of an end effector attached on a sixth axis are given, a predetermined amount of offset exists between a sixth axis and a fourth axis, and the method includes sequentially determining a point of interest, which is a point on a circumference of a circle having the predetermined amount as a radius, around a first intersection point, on a plane which includes the first intersection point which is an intersection point of the sixth axis and the fifth axis and the plane which is orthogonal to the sixth axis, calculating a second intersection point, which is an intersection point of the fourth axis and the third axis, when it is assumed that the point of interest is an intersection point of the fifth axis and the fourth axis, calculating an inner product value of a first vector directed from the calculated second intersection point to the point of interest and a second vector directed from the point of interest to the first intersection point, and estimating that the point of interest, when an absolute value of the inner product value is less than or equal to a predetermined threshold, is an intersection point of the fifth axis and the fourth axis.

CROSS REFERENCE TO RELATED APPLICATIONS

This application claims the benefit of priority to Japanese PatentApplication No. 2018-067235 filed on Mar. 30, 2018. The entire contentsof this application are hereby incorporated herein by reference.

BACKGROUND OF THE INVENTION 1. Field of the Invention

The present disclosure relates to a robot control apparatus, and amethod and program for obtaining an angle of a joint of a robot.

2. Description of the Related Art

A Programmable Universal Machine for Assembly (PUMA)-type robot isgenerally known as a robot having an axis configuration in whichrotation axes of three joints of hand tips intersect at one point.

The analysis of inverse kinematics of a PUMA-type robot is thought to beanalytically determined. However, when there is an offset in a wristportion of the robot, a processing load of calculation of the inversekinematics is heavy and it takes time. For this reason, a high-speedprocessor is required to perform real-time calculation suitable for therobot to operate smoothly.

SUMMARY OF THE INVENTION

An example embodiment of the present disclosure is a robot controlapparatus for obtaining an angle of each joint of a 6-axis verticalarticulated robot when a position and a posture of an end effectorattached on a sixth axis are given, wherein, in the 6-axis verticalarticulated robot, the sixth axis of a sixth joint and a fifth axis of afifth joint perpendicularly intersect each other, the fifth axis and afourth axis of a fourth joint perpendicularly intersect each other, thefourth axis and a third axis of a third joint perpendicularly intersecteach other, and there is a predetermined amount of offset between thesixth axis and the fourth axis. The robot control apparatus includes adeterminer to sequentially determine a point of interest, which is apoint on a circumference of a circle having the predetermined amount asa radius, around a first intersection point on a plane including thefirst intersection point which is an intersection point of the sixthaxis, and the fifth axis and the plane which is orthogonal to the sixthaxis. The robot control apparatus includes a first calculator tocalculate a second intersection point, which is an intersection point ofthe fourth axis and the third axis, when it is assumed that the point ofinterest is an intersection point of the fifth axis and the fourth axis,and a second calculator to calculate an inner product value of a firstvector directed from the second intersection point calculated by thefirst calculator to the point of interest and a second vector directedfrom the point of interest to the first intersection point. The robotcontrol apparatus includes an estimator to estimate that the point ofinterest, when an absolute value of the inner product value is less thanor equal to a predetermined threshold, is an intersection point of thefifth axis and the fourth axis.

The above and other elements, features, steps, characteristics andadvantages of the present disclosure will become more apparent from thefollowing detailed description of the example embodiments with referenceto the attached drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a schematic view showing a configuration of a robot system ofa robot according to an example embodiment of the present disclosure.

FIG. 2 is a front view of a robot according to an example embodiment ofthe present disclosure in a state in which a second arm is lowered.

FIG. 3 is a front view of a robot according to an example embodiment ofthe present disclosure in a state in which the second arm is raised.

FIG. 4 is a view showing a link coordinate system of a robot accordingto an example embodiment of the present disclosure.

FIG. 5 is a perspective view of a modeled robot according to an exampleembodiment of the present disclosure in a case in which the robot has acertain posture.

FIG. 6 is a perspective view of a modeled robot according to an exampleembodiment of the present disclosure in a case in which the robot hasanother posture.

FIG. 7 is a perspective view of a modeled reference robot in a case inwhich the robot has a certain posture.

FIG. 8 is a perspective view of the modeled reference robot in a case inwhich the robot has another posture.

FIG. 9 is a view showing a relationship between rotation axes of jointsof the reference robot of FIG. 8.

FIG. 10 is a view for describing an algorithm of analysis of inversekinematics of the reference robot of FIG. 8.

FIG. 11 is a view showing a relationship between rotation axes of jointsof the robot according to the example embodiment of FIG. 6.

FIG. 12 is a view for describing an algorithm of analysis of inversekinematics of a robot according to an example embodiment of the presentdisclosure.

FIG. 13 is a block diagram showing a configuration of a robot systemaccording to an example embodiment of the present disclosure.

FIG. 14 is a functional block diagram of a robot control apparatusaccording to an example embodiment of the present disclosure.

FIG. 15 is a flowchart executed by a robot control apparatus accordingto an example embodiment of the present disclosure.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Hereinafter, robot systems including robot control apparatuses accordingto example embodiments of the present disclosure will be described.

In the following description, unless otherwise indicated, points referto points in a world coordinate system (robot coordinate system).

First, a configuration of a robot system 1 according to a firstembodiment will be described with reference to FIGS. 1 to 3. FIG. 1 is aschematic view showing a configuration of the robot system 1 accordingto the present embodiment. FIG. 2 is a front view of a robot accordingto the present embodiment in a state in which a second arm (to bedescribed below) is lowered. FIG. 3 is a front view of the robotaccording to the present embodiment in a state in which the second armis raised.

A robot R according to the present embodiment is an articulated robotwhich is used for assembling or manufacturing predetermined products andis installed and used, for example, on an assembly line or a productionline.

As shown in FIGS. 2 and 3, the robot R includes a plurality of jointsand a plurality of arms. In an example of the present embodiment, therobot R includes six joints 11 to 16, a first arm 21, a second arm 22,and an end effector attaching portion 18. Although not shown in thedrawings, each of the joints includes a motor, a decelerator connectedto the motor, a position detecting mechanism for detecting a rotationalposition of the motor, and a circuit board on which the motor and theposition detecting mechanism are electrically connected.

Further, the robot R includes a support member B constituting a base endportion of the robot R. The support member B is relatively rotatablyconnected to a first joint 11. The first arm 21 and the second arm 22are formed to have a thinly elongated longitudinal shape. Specifically,each of the first arm 21 and the second arm 22 is formed to have athinly elongated cylindrical shape.

In the robot R, the following fixed or connected relationships areestablished.

(i) The first joint 11 and a second joint 12 are relatively rotatablyconnected to each other.

(ii) The second joint 12 and a first end 211 of the first arm 21 arefixed to each other.

(iii) A second end 212 of the first arm 21 and a third joint 13 arefixed to each other.

(iv) The third joint 13 and a fourth joint 14 are relatively rotatablyconnected to each other.

(v) The fourth joint 14 and a first end 221 of the second arm 22 arerelatively rotatably connected to each other.

(vi) A second end 222 of the second arm 22 and a fifth joint 15 arefixed to each other.

(vii) The fifth joint 15 and a sixth joint 16 are relatively rotatablyconnected to each other.

(viii) The sixth joint 16 and the end effector attaching portion 18 arerelatively rotatably connected to each other.

An end effector such as a hand or a tool may be attached to the endeffector attaching portion 18.

Hereinafter, in order to describe a mechanism of the robot R accordingto the present embodiment, Denavit-Hartenberg (DH) parameters are shownas link parameters of the robot R according to the present embodiment,and forward kinematics of the robot R according to the presentembodiment will be described with reference to FIG. 4 as a reference.FIG. 4 is a view showing a link coordinate system of the robot Raccording to the embodiment.

In the robot R shown in FIGS. 2 and 3, when it is assumed thatintersection points of rotation axes (an intersection point of Z-axes)set by the respective joints are intersection points O₁ to O₅ and aninstallation position of an end effector is a point O₆, Link1 to Link6coordinate systems, which are link coordinate systems having points O₁to O₆ as original points, are shown in FIG. 4. In FIG. 4, the Link1 toLink6 coordinate systems are denoted by ΣL₁ to ΣL₆, respectively.Further, the link shown in FIG. 4 corresponds to a posture of the robotR in FIG. 3.

As shown in FIG. 4, in the robot R according to the present embodiment,there is an offset of an interlink distance d₅ between the intersectionpoint O₄ and the intersection point O₅.

The DH parameters of the robot R are as shown in Table 1 below. In FIG.4, since an equation, d₂=d₃, is established with respect to interlinkdistances d₂ and d₃, the interlink distances d₂ and d₃ may be zero onthe DH parameters.

TABLE 1 Interlink Interlink Link Link Twist Coordinate Distance d Angleθ Length a Angle α System [mm] [deg] [mm] [deg] Link1 d₁ 90 + θ₁ 0 90Coordinate System (ΣL₁) Link2 0 90 + θ₂ a₂ 0 Coordinate System (ΣL₂)Link3 0 90 + θ₃ 0 90 Coordinate System (ΣL₃) Link4 d₄ θ₄ 0 −90Coordinate System (ΣL₄) Link5 d₅ θ₅ 0 90 Coordinate System (ΣL₅) Link6d₆ θ₆ 0 0 Coordinate System (ΣL₆)

A homogeneous transformation matrix T₁ for the Link1 coordinate systemΣL₁ from a world coordinate diameter is obtained using the DH parametersas follows.

$\begin{matrix}{T_{1} = {\begin{bmatrix}{\cos\left( {\theta_{1} + 90} \right)} & {- {\sin\left( {\theta_{1} + 90} \right)}} & 0 & 0 \\{\sin\left( {\theta_{1} + 90} \right)} & {\cos\left( {\theta_{1} + 90} \right)} & 0 & 0 \\0 & 0 & 0 & d_{1} \\0 & 0 & 0 & 1\end{bmatrix} \cdot}} \\{\begin{bmatrix}1 & 0 & 0 & 0 \\0 & {\cos\left( \alpha_{1} \right)} & {- {\sin\left( \alpha_{1} \right)}} & 0 \\0 & {\sin\left( \alpha_{1} \right)} & {\cos\left( \alpha_{1} \right)} & 0 \\0 & 0 & 0 & 1\end{bmatrix}} \\{= \begin{bmatrix}{- {\sin\left( \theta_{1} \right)}} & 0 & {\cos\left( \theta_{1} \right)} & 0 \\{\cos\left( \theta_{1} \right)} & 0 & {\sin\left( \theta_{1} \right)} & 0 \\0 & 1 & 0 & d_{1} \\0 & 0 & 0 & 1\end{bmatrix}}\end{matrix}$

Hereinafter, when it is assumed that equations, S_(n)=sin(θ_(n)) andC_(n)=cos(θ_(n)), are established, homogeneous transformation matrixesT₁, T₂, T₃, T₄, T₅, and T₆ among the respective link coordinate systemsare as follows.

${T_{1} = \begin{bmatrix}{- S_{1}} & 0 & C_{1} & 0 \\C_{1} & 0 & S_{1} & 0 \\0 & 1 & 0 & d_{1} \\0 & 0 & 0 & 1\end{bmatrix}}\mspace{14mu}$ $T_{2} = \begin{bmatrix}{- S_{2}} & {- C_{2}} & 0 & {{- a_{2}}S_{2}} \\C_{2} & {- S_{2}} & 0 & {a_{2}C_{2}} \\0 & 0 & 1 & 0 \\0 & 0 & 0 & 1\end{bmatrix}$ $T_{3} = \begin{bmatrix}{- S_{3}} & 0 & C_{3} & 0 \\C_{3} & 0 & S_{3} & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 0 & 1\end{bmatrix}$ $T_{4} = \begin{bmatrix}C_{4} & 0 & {- S_{4}} & 0 \\S_{4} & 0 & C_{4} & 0 \\0 & {- 1} & 0 & d_{4} \\0 & 0 & 0 & 1\end{bmatrix}$ $T_{5} = \begin{bmatrix}C_{5} & 0 & {- S_{5}} & 0 \\S_{5} & 0 & {- C_{5}} & 0 \\0 & 1 & 0 & d_{5} \\0 & 0 & 0 & 1\end{bmatrix}$ $T_{6} = \begin{bmatrix}C_{6} & {- S_{6}} & 0 & 0 \\S_{6} & C_{6} & 0 & 0 \\0 & 0 & 1 & d_{6} \\0 & 0 & 0 & 1\end{bmatrix}$

From the above, a homogeneous transformation matrix T_(ee) of the endeffector is calculated from the homogeneous transformation matrixesamong the respective link coordinate systems using the followingEquation 1. By applying Equation 1, a position and a posture of the endeffector may be obtained when the rotation angle of each joint is known.Tee=T ₁ T ₂ T ₃ T ₄ T ₅ T ₆  (1)

Next, a calculation algorithm (hereinafter, simply referred to as “analgorithm according to the present embodiment”) of analysis of inversekinematics of the robot R according to the present embodiment will bedescribed with reference to FIGS. 5 to 12.

Since the algorithm according to the present embodiment is based on acalculation algorithm of analysis of inverse kinematics of a referencerobot, the reference robot will be described below in addition to thedescription of the robot R according to the present embodiment. Unlikethe robot R according to the present embodiment, the reference robotrefers to a robot (commonly known as a Programmable Universal Machinefor Assembly (PUMA)-type robot) having no offset in a wrist, that is, arobot in which the interlink distance d₃ in FIG. 4 is zero.

First, the robot R according to the present embodiment is modeled asshown in FIGS. 5 and 6.

FIGS. 5 and 6 are perspective views of a robot model which is modeledbased on the joints and the links of the robot R according to thepresent embodiment and represent different postures of the robot R. Theposture of the robot model according to the present embodiment in FIG. 5is a posture similar to that of the robot R shown in FIG. 2.

In FIGS. 5 and 6, the robot model according to the present embodimentincludes six joints J1 to J6, and axes (rotation axes) of the six jointsJ1 to J6 are axes Ax₁ to Ax₆, respectively. In the actual robot R shownin FIGS. 2 and 3, the joints J1 to J6 rotating around the axes Ax₁ toAx₆ correspond to the joints 11 to 16, respectively.

An intersection point of the axis Ax₃ and the axis Ax₄ is anintersection point O₃, an intersection point of the axis Ax₄ and theaxis Ax₅ is an intersection point O₄, and an intersection point of theaxis Ax₅ and the axis Ax₆ is an intersection point O₅. A point O₆ is aninstallation position of an end effector on the axis Ax₆.

As shown in FIG. 5, in the robot model according to the presentembodiment, an offset d₅ is provided between the axis Ax₄ and the axisAx₆.

Meanwhile, FIGS. 7 and 8 are perspective views of a robot model(hereinafter, referred to as “a reference robot model”) which is modeledbased on the joints and the links of the reference robot and representdifferent postures of the robot R. FIG. 7 shows the same posture as thatin FIG. 5, and FIG. 8 shows the same posture as that in FIG. 6.

In FIGS. 7 and 8, the reference robot model includes six joints J1 toJ6, and axes of the six joints J1 to J6 are axes Ax₁ to Ax₆ which arethe same as those in the robot model according to the presentembodiment. However, unlike the robot model according to the presentembodiment, in the reference robot model, there is no offset between theaxis Ax₄ and the axis Ax₆, and an intersection point O₄ and anintersection point O₅ coincide with each other.

Next, a calculation algorithm of analysis of inverse kinematics of thereference robot model will be described with reference to FIGS. 9 and10. FIG. 9 is a view showing a relationship between the rotation axes ofthe joints of the reference robot model of FIG. 8. FIG. 10 is a view fordescribing an algorithm of the analysis of the inverse kinematics of thereference robot model of FIG. 8.

FIG. 9 shows a relationship between the axes of the respective joints ofthe reference robot model shown in FIG. 8 and intersection points.Further, an intersection point O₁ is an intersection point of an axisAx₁ and an axis Ax₂.

The reference robot model, which is a model of a PUMA-type robot, hasthe following features.

In the reference robot model, regardless of rotation angles θ₁, θ₂, θ₃,θ₅, and θ₆ of the joints J1, J2, J3, J5, and J6, unless the joint J4rotates, intersection points O₁, O₃, O₄, and O₅ and a point O₆ are onthe same plane. The same plane is hereinafter referred to as “an armplane.” That is, as the joint J1 rotates, the arm plane itself rotatesaround the joint J1. However, unless the joint J4 rotates, theintersection point O₁, O₃, O₄, and O₅ and the point O₆ are always on thearm plane. When the rotation angle θ₄ of the joint J4 is changed, thepoint O₆ at an installation position of the end effector deviates fromthe arm plane.

In the reference robot model, even when the rotation angle θ₄ of thejoint J4 is changed and the end effector deviates from the arm plane, aposition of the intersection point O₄, which is an intersection point ofthe axis Ax₄, the axis Ax₅, and the axis Ax₆ on the arm plane, may becalculated when a position and a posture of the end effector are given.That is, since the point, which is moved by a distance d6 along the axisAx₆ from the point O₆ at the installation position of the end effector,becomes a common point between the intersection point O₄ and theintersection point O₅, the position of the intersection point O₄ may beeasily calculated.

After the position of the intersection point O₄ is calculated, therotation angles of the joints J1 to J3 may be calculated from geometryon the arm plane as follows.

FIG. 10 is a view showing a positional relationship between theintersection point O₄, O₃, and O₁ on the arm plane. In FIG. 10, a pointP4 is an intersection point of a perpendicular line drawn from theintersection point O₁ with respect to a perpendicular line drawn fromthe intersection point O₄ onto a xy plane of a world coordinate system.

First, referring to FIG. 9, the rotation angle θ₁ of the joint J1 may becalculated according to the following Equation 2 on the basis of O₄·xand O₄·y, which are x and y coordinates of the world coordinate systemof the calculated intersection point O₄.

$\begin{matrix}{\theta_{1} = {\tan^{- 1}\frac{O_{4} \cdot y}{O_{4} \cdot x}}} & (2)\end{matrix}$

Further, in the arm plane shown in FIG. 10, a distance Lx between theintersection point O₁ and the point P4 and a distance Ly between theintersection point O₁ and the intersection point O₄ may be calculatedaccording to the following Equations 3 and 4 using O₄·z, which iscoordinates of the world coordinate system of the intersection point O₄.Lx=√{square root over (O ₄ ·x ² +O ₄ ·y ²)}  (3)Ly=√{square root over (O ₄ ·x ² +O ₄ ·y ²+(O ₄ ·z−d ₁)²)}  (4)

Therefore, since angles θ_(a), θ_(b), and θ_(c) may be geometricallycalculated from FIG. 10, rotation angles θ₂ and θ₃ of the joint J2 andJ3 may be calculated.

Next, an algorithm in the present embodiment will be described withreference to FIGS. 11 and 12. FIG. 11 is a view showing a relationshipbetween the rotation axes of the joints of the robot R according to thepresent embodiment of FIG. 6. FIG. 12 is a view for describing analgorithm of analysis of inverse kinematics of the robot R according tothe present embodiment.

In the robot model according to the present embodiment, since there isan offset between the axis Ax₄ and the axis Ax₆, the intersection pointO₄ and the intersection point O₅ do not coincide. When the position andthe posture of the end effector are given, since an interlink distanced6 is known, the intersection point O₅ may be obtained from the pointO₆, but the intersection point O₄ may not be obtained from theintersection point O₅. This is because it is not possible to know inwhich direction the axis Ax₅ is directed.

Therefore, convergence calculation is performed while estimation isperformed on the position of the intersection point O₄. It is consideredthat the intersection point O₄ is positioned on a circumference of acircle (the circle is indicated by a circle C5 in FIGS. 11 and 12)having a radius d₅ around an original point on a xy plane of a linkcoordinate system with the axis Ax₅ as a z axis z₅ (in FIG. 12).

As shown in FIG. 12, a point O₄t on the circumference of the circle C5is determined as a point of interest, and a candidate point O₃t, whichis an intersection point O₃ when the point O₄t of interest is taken asthe intersection point O₄, is obtained in the same manner as thealgorithm of the reference robot model. In the robot model according tothe present embodiment, since the arm plane is also determined by theintersection points O₁, O₃, and O₄ in the same manner as that in thereference robot model, the candidate point O₃t may be obtained accordingto the geometric relationship as shown in FIG. 10.

Since the axis Ax₄ and the axis Ax₅ are orthogonal to each other due toconstraint conditions of the links of the robot R, it may be determinedthat the point O₄t of interest is the intersection point O₄ when aninner product value of a vector (an example of a first vector) directedfrom the candidate point O₃t to the point O₄t of interest and a vector(an example of a second vector) directed from the point O₄t of interestto the intersection point O₅ is zero. Therefore, the point O₄t ofinterest is sequentially moved on the circumference of the circle C5,and processing is performed so as to search for a point at which theinner product value becomes zero. In addition, in consideration of acalculation time, when an absolute value of the inner product value isless than or equal to a predetermined threshold as a convergencecondition, it may be estimated that the point O₄t of interest is theintersection point O₄.

After coordinates of the intersection point O₄ are specified, therotation angles θ₁ to θ₃ of the joints J1 to J3 may be calculated usingthe same algorithm as that used in the reference robot model.

Next, a configuration of a robot control apparatus 3 according to thepresent embodiment will be described with reference to FIG. 13.

FIG. 13 is a block diagram showing the configuration of the robot systemaccording to the present embodiment.

As shown in FIG. 13, the robot control apparatus 3 includes a controller31, a storage 32, and a communication unit 33.

The controller 31 includes a central processing unit (CPU), a read onlymemory (ROM), and a random access memory (RAM) The CPU receives a robotprogram from an external information processing apparatus (not shown),stores the robot program in the storage 32, and loads and executes therobot program in the RAM. Each function which will be described below isrealized by the robot program executed by the CPU of the controller 31.The algorithm according to the present embodiment is included in therobot program.

The controller 31 generates a control pulse for operating each of thejoints 11 to 16 by executing the robot program.

The storage 32 is a mass storage device such as a hard disk drive (HDD)or a solid state drive (SSD). The robot program, the above-described DHparameters (or the link parameters), and execution log data, which is anexecution record of the robot program, are stored in the storage 32.

The communication unit 33 includes a communication interface circuit,which communicates with another communication device and receives therobot program. The communication unit 33 is communicably connected to,for example, a robot teaching device (not shown) operated by anoperator, and receives the robot program from the robot teaching device.

As shown in FIG. 13, the robot R includes a motor driving circuit 101and a motor 102, which are embedded into each of the joints 11 to 16.The motor driving circuit 101 drives the motor 102 in response to acommand from the controller 31.

Next, functions realized by the robot program which is executed by thecontroller 31 of the robot control apparatus 3 will be described withreference to FIG. 14. FIG. 14 is a functional block diagram of the robotcontrol apparatus 3 according to the present embodiment.

As shown in FIG. 14, blocks for functions realized by the robot controlapparatus 3 include a point of interest determination unit 311, a firstcalculation unit 312, a second calculation unit 313, and an estimationunit 314.

As described above, the robot R according to the present embodiment is a6-axis vertical articulated robot in which an axis Ax₆ of a joint J6 andan axis Ax₅ of a joint J5 perpendicularly intersect each other, the axisAx₅ and an axis Ax₄ of a joint J4 perpendicularly intersect each other,the axis Ax₄ and an axis Ax₃ of a joint J3 perpendicularly intersecteach other, and there is a predetermined amount of offset between theaxis Ax₆ and the axis Ax₄. The controller 31 of the robot controlapparatus 3 according to the present embodiment executes the robotprogram to obtain an angle of each joint of the robot R when theposition and the posture of the end effector are given to the axis Ax₆.

The point of interest determination unit 311 has a function ofsequentially determining a point O₄t of interest, which is a point on acircumference of a circle having the interlink distance d₅ (an exampleof a predetermined amount) as a radius, around the intersection pointO₅, on a plane which includes the intersection point O₅ (an example of afirst intersection point) which is an intersection point of the axis Ax₆and the axis Ax₅ and is orthogonal to the axis Ax₆. The plane, whichincludes the intersection point O₅ and is orthogonal to the axis Ax₆,refers to a xy plane of the link coordinate system with the axis Ax₅ asa Z axis. The controller 31 determines, for example, a point which issequentially moved on the circumference of the circle C5 shown in FIGS.11 and 12 in the same direction at a predetermined interval (pitch) asthe point O₄t of interest.

The first calculation unit 312 has a function of calculating a candidatepoint O₃t (an example of a second intersection point), which is anintersection point of the axis Ax₄ and the axis Ax₃, when it is assumedthat the point O₄t of interest determined by the point of interestdetermination unit 311 is the intersection point O₄ of the axis Ax₅ andthe axis Ax₄. As shown in FIG. 10, the candidate point O₃t, which is theintersection point O₃ when it is assumed that the intersection point O₄is the point O₄t of interest, may be obtained based on the geometricrelationship on the arm plane.

The second calculation unit 313 has a function of calculating an innerproduct value of a first vector directed from the candidate point O₃tcalculated by the first calculation unit 312 to the point O₄t ofinterest and a second vector directed from the point O₄t of interest tothe intersection point O₅. Since coordinates of the point O₄t ofinterest, the candidate point O₃t, and the intersection point O₅ in therobot coordinate system are known, the inner product value may becalculated.

The estimation unit 314 has a function of estimating that the point O₄tof interest when an absolute value of the inner product value calculatedby the second calculation unit 313 is less than or equal to apredetermined threshold is the intersection point O₄ of the axis Ax₅ andthe axis Ax₄.

Next, processing of the robot control apparatus 3, on which thealgorithm according to the present embodiment is implemented, will bedescribed with reference to a flowchart of FIG. 15.

In the robot program, values of the position and the posture of the endeffector, which are required from teaching information of the robot, aredescribed. The controller 31 of the robot control apparatus 3 calculatesthe intersection point O₅ of the axis Ax₆ and the axis Ax₅ from therequired values of the position and the posture of the end effector(S10). Since the point O₆ at the installation position of the endeffector, the direction of the axis Ax₆, and the interlink distance d6are known, the controller 31 may calculate coordinates of theintersection point O₅.

Next, the controller 31 determines the point O₄t of interest, which is apoint on a circumference of a circle having the interlink distance d₅ asa radius, around the intersection point O₅ on a plane which includes theintersection point O₅ (an example of the first intersection point) andthe plane which is orthogonal to the axis Ax₆ (S12). A setting startposition of the point O₄t of interest on the circumference of the circleC5 may be arbitrarily determined.

When the point O₄t of interest is determined, the controller 31calculates the candidate point O₃t of the intersection point O₃ on thebasis of the geometric relationship on the arm plane shown in FIG. 10(S14).

Next, the controller 31 calculates an inner product value of a vectordirected from the candidate point O₃t to the point O₄t of interest and avector directed from the point O₄t of interest to the intersection pointO₅ (S16). When an absolute value of the obtained inner product value isless than or equal to a predetermined threshold (NO in operation S18),the controller 31 moves the point O₄t of interest (i.e., changes thepoint O₄t of interest) on the circumference of the circle C5 at apredetermined interval (pitch) and re-performs processing of operationsS12 to S18.

When the inner product value obtained in operation S16 is less than orequal to the predetermined threshold (YES in operation S18), it may bedetermined that a line connecting the candidate point O₃t to the pointO₄t of interest and a line connecting the point O₄t of interest to theintersection point O₅ are substantially orthogonal to each other and theconstraint conditions of the links are satisfied. Therefore, thecontroller 31 determines that the candidate point O₃t and the point O₄tof interest when the inner product value is less than or equal to thepredetermined threshold are the intersection point O₃ and theintersection point O₄ (S20).

Next, the controller 31 calculates the rotation angles θ₁ to θ₃ of thejoints J1 to J3 on the basis of the geometric relationship on the armplane shown in FIG. 10 based on the intersection point O₃ and O₄determined in operation S20 (S22).

As described above, according to the robot control apparatus 3 accordingto the present embodiment, even when there is an offset (i.e., theinterlink distance d₅ in FIG. 4) in the wrist portion of the robot R,the rotation angle of each joint may be calculated by a relativelysimple calculation when the position and the posture of the end effectorare given. That is, it is possible to reduce a calculation load ofinverse kinematics of the robot having an offset in the wrist portion ofthe robot R.

Although the embodiments of the robot control apparatus of the presentdisclosure have been described, the present disclosure is not limited tothe above embodiments.

According to the above description, it is understood by those skilled inthe art that a program for causing a computer to implement the functionsdescribed in the functional block diagram of FIG. 14 and a computerreadable storage medium (including a nonvolatile storage medium), inwhich the program is recorded, are disclosed.

Features of the above-described preferred embodiments and themodifications thereof may be combined appropriately as long as noconflict arises.

While preferred embodiments of the present invention have been describedabove, it is to be understood that variations and modifications will beapparent to those skilled in the art without departing from the scopeand spirit of the present invention. The scope of the present invention,therefore, is to be determined solely by the following claims.

What is claimed is:
 1. A robot control apparatus for obtaining an angleof each joint of a 6-axis vertical articulated robot when a position anda posture of an end effector attached on a sixth axis are given,wherein, in the 6-axis vertical articulated robot, the sixth axis of asixth joint and a fifth axis of a fifth joint perpendicularly intersecteach other, the fifth axis and a fourth axis of a fourth jointperpendicularly intersect each other, the fourth axis and a third axisof a third joint perpendicularly intersect each other, and there is apredetermined amount of offset between the sixth axis and the fourthaxis, the robot control apparatus comprising: determination circuitry tosequentially determine a point of interest, which is a point on acircumference of a circle having the predetermined amount of offset as aradius, around a first intersection point, on a plane which includes thefirst intersection point which is an intersection point of the sixthaxis and the fifth axis and the plane which is orthogonal to the sixthaxis; a first calculation circuitry to calculate a second intersectionpoint, which is an intersection point of the fourth axis and the thirdaxis, when it is assumed that the point of interest is an intersectionpoint of the fifth axis and the fourth axis; a second calculationcircuitry to calculate an inner product value of a first vector directedfrom the second intersection point calculated by the first calculationcircuitry to the point of interest and a second vector directed from thepoint of interest to the first intersection point; and estimationcircuitry to estimate that the point of interest, when an absolute valueof the inner product value is less than or equal to a predeterminedthreshold, is an intersection point of the fifth axis and the fourthaxis; wherein the sequentially determination of the point of interest,the calculation of the second intersection point, the calculation of theinner product value of the first vector, and the estimation that thepoint of interest is the intersection point of the fifth axis and thefourth axis are all performed in real-time.
 2. The robot controlapparatus of claim 1, wherein the determination circuitry, the firstcalculation circuitry, the second calculation circuitry, and theestimation circuitry are defined by a controller.
 3. A method ofobtaining an angle of each joint of a 6-axis vertical articulated robotwhen a position and a posture of an end effector attached on a sixthaxis are given, wherein, in the 6-axis vertical articulated robot, thesixth axis of a sixth joint and a fifth axis of a fifth jointperpendicularly intersect each other, the fifth axis and a fourth axisof a fourth joint perpendicularly intersect each other, the fourth axisand a third axis of a third joint perpendicularly intersect each other,and there is a predetermined amount of offset between the sixth axis andthe fourth axis, the method comprising: sequentially determining a pointof interest; which is a point on a circumference of a circle having thepredetermined amount of offset as a radius, around a first intersectionpoint, on a plane which includes the first intersection point which isan intersection point of the sixth axis, and the fifth axis and theplane which is orthogonal to the sixth axis; calculating a secondintersection point which is an intersection point of the fourth axis andthe third axis when it is assumed that the point of interest is anintersection point of the fifth axis and the fourth axis; calculating aninner product value of a first vector directed from the secondintersection point to the point of interest and a second vector directedfrom the point of interest to the first intersection point; andestimating that the point of interest, when an absolute value of theinner product value is less than or equal to a predetermined threshold,is an intersection point of the fifth axis and the fourth axis; whereinthe sequentially determining the point of interest, the calculating thesecond intersection point, the calculating the inner product value ofthe first vector, and the estimating that the point of interest is theintersection point of the fifth axis and the fourth axis are allperformed in real-time.
 4. A non-transitory computer-readable mediumcontaining a computer program that causes a computer to perform a methodof obtaining an angle of each joint of a 6-axis vertical articulatedrobot when a position and a posture of an end effector attached on asixth axis are given, wherein, in the 6-axis vertical articulated robot,the sixth axis of a sixth joint and a fifth axis of a fifth jointperpendicularly intersect each other, the fifth axis and a fourth axisof a fourth joint perpendicularly intersect each other, the fourth axisand a third axis of a third joint perpendicularly intersect each other,and there is a predetermined amount of offset between the sixth axis andthe fourth axis, the method comprising: sequentially determining a pointof interest, which is a point on a circumference of a circle having thepredetermined amount of offset as a radius, around a first intersectionpoint, on a plane which includes the first intersection point which isan intersection point of the sixth axis and the fifth axis and the planewhich is orthogonal to the sixth axis; calculating a second intersectionpoint which is an intersection point of the fourth axis and the thirdaxis when it is assumed that the point of interest is an intersectionpoint of the fifth axis and the fourth axis; calculating an innerproduct value of a first vector directed from the second intersectionpoint to the point of interest and a second vector directed from thepoint of interest to the first intersection point; and estimating thatthe point of interest, when an absolute value of the inner product valueis less than or equal to a predetermined threshold, is an intersectionpoint of the fifth axis and the fourth axis; wherein the sequentiallydetermining the point of interest, the calculating the secondintersection point, the calculating the inner product value of the firstvector, and the estimating that the point of interest is theintersection point of the fifth axis and the fourth axis are allperformed in real-time.